/****************************************************************************
*  Title: corner_detect.sc
*  Author: Ashish Daga
*  Date: 04/16/2011
*  Description: takes in an 8bpp image array row by row over input c_queue
*			    and uses a Sobel mask for computing the differential gradients and then the Harris Response is 
			    computed. Response above the threshold are marked as corners whose locations are stored.
*
****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>


// define image size, idea being from a hw camera this would be fixed
#define IMG_ROWS 480
#define IMG_CLMS 640

double Gx[IMG_ROWS][IMG_CLMS];  //gradients used for calculating the response.
double Gy[IMG_ROWS][IMG_CLMS];
double Gxy[IMG_ROWS][IMG_CLMS];

int PointX[IMG_ROWS];
int PointY[IMG_CLMS];
int Points_Count;


import "c_queue";


/*
* Receives imput image from c_queue
*/
behavior receiveImage(i_receiver InQueue, out unsigned char image_buffer[IMG_ROWS][IMG_CLMS]) 
{
	// image buffers these are currently dependent on the size and format of the image used
	unsigned char row_buffer_1[IMG_CLMS]; // this file is 4x4 since there are 3 bytes/pixel -> 1536 bytes per row
	int row, a, j;

	void main(void) 
	{				
		printf("Reading Image\n");
		
		//********************
		// Load image from c_queue into memory
		//********************
		
		// read image from c_queue and load into image buffer
		for(row = 0; row < IMG_ROWS; row++) {

			// Load row with a row of the image
			InQueue.receive(row_buffer_1, (IMG_CLMS));	
			
			// place row into image buffer
			for(a = 0; a < (IMG_CLMS); a++){
				image_buffer[row][a] =  row_buffer_1[a]; 	
			}			
			
			// generate grayscale image matrix from color RGM input image
//			for(j = 0; j < (IMG_CLMS); j = j++)
//			{
//				// create 8 bpp grayscale image with weight of .3R + .59G + .11B
//				image8bpp[row][j/3] = (unsigned char) (0.3 * (row_buffer_1[j]) + 0.59 * (row_buffer_1[j+1]) + 0.11 * (row_buffer_1[j+2]));
//			}
		}		
		printf("Finished reading image into memory.  Starting Image Processing . . . .\n");
		
	}
};


/*
* Calculate image differentials
*/
behavior preProcessImage( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS], out double diffxy[IMG_ROWS][IMG_CLMS])
{

	int masksize = 3;
	int convolveX[3][3] = {{-1, 0, 1}, {-1, 0, 1}, {-1, 0, 1}};
	int convolveY[3][3] = {{-1, -1, -1}, {0, 0, 0}, {1, 1, 1}};
	int x, y, x_1, y_1, posx, posy, imPosx, imPosy;
	int width, height;

	double diffx[IMG_ROWS][IMG_CLMS];
	double diffy[IMG_ROWS][IMG_CLMS];		
	double valx, valy;

	void main(void) 
	{
	
		width = IMG_CLMS;
		height = IMG_ROWS;
		//********************
		// Apply x, y, xy gradient convolution  - COmmon b.w 
		//********************			
		for(x = masksize / 2; x < width - (masksize / 2); x++) {  //columns
			for(y= masksize / 2; y < height- (masksize / 2); y++) { // rows
				valx = 0;
				valy = 0;
				for( x_1 = 0; x_1 < masksize; x_1++) {
					for( y_1 = 0; y_1 < masksize; y_1++) {
						imPosx = (x + (x_1 - (masksize / 2)));
						imPosy = (y + (y_1 - (masksize / 2)));
						
						valx += ((image8bpp[imPosy][imPosx]) * convolveX[y_1][x_1]);
						valy += ((image8bpp[imPosy][imPosx]) * convolveY[y_1][x_1]);
					}
				}
				diffx[y][x] = valx * valx;  // horizontial gradient
				diffy[y][x] = valy * valy;  // vertical gradient
				diffxy[y][x] =  valx * valy;  // xy gradient
			}
		}	
	}
};


/*
* Performs sobel edge detection
*/
// could split into sobel-x and sobel-y if placing on seperate hardware
// not done here, since this will be mapped to a single PE there is no sense in replicating loops and counters
behavior sobelEdge( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS], out double sobelG[IMG_ROWS][IMG_CLMS])
{

	int masksize = 3;
	int sobelX[3][3] = {{-1, 0, 1}, {-2, 0, 2}, {-1, 0, 1}};
	int sobelY[3][3] = {{1, 2, 1}, {0, 0, 0}, {-1, -2, -1}};

	/*
	float kernel = {0.0286864862,
			0.102712758,	
			0.220796719,	
			0.28495878,	
			0.220796719,	
			0.102712758,	
			0.0286864862};	*/


	

	int x, y, x_1, y_1, posx, posy, imPosx, imPosy;
	int width, height;

	//double Gx[IMG_ROWS][IMG_CLMS];
	//double Gy[IMG_ROWS][IMG_CLMS];
	//double Gxy[IMG_ROWS][IMG_CLMS];				
	double valx, valy;

	void main(void) 
	{
	
		width = IMG_CLMS;
		height = IMG_ROWS;
		//********************
		// Apply x, y, xy gradient convolution
		//********************			
		for(x = masksize / 2; x < width - (masksize / 2); x++) {  //columns
			for(y= masksize / 2; y < height- (masksize / 2); y++) { // rows
				valx = 0;
				valy = 0;
				for( x_1 = 0; x_1 < masksize; x_1++) {
					for( y_1 = 0; y_1 < masksize; y_1++) {
						imPosx = (x + (x_1 - (masksize / 2)));
						imPosy = (y + (y_1 - (masksize / 2)));
						
						valx += ((image8bpp[imPosy][imPosx]) * sobelX[y_1][x_1]);
						valy += ((image8bpp[imPosy][imPosx]) * sobelY[y_1][x_1]);
						//printf("valx %f\n", valx);
					}
				}
				
				sobelG[y][x] =  sqrt(valx * valx + valy * valy);  // gradient magnitude
				
				// can approximate magniture and normalization for edge (Myler method)
				Gx[y][x] = valx * valx;  // horizontial gradient
				Gy[y][x] = valy * valy;  // vertical gradient				
				Gxy[y][x] = valx * valy;  // xy 
				//if (Gxy>255) Gxy=255;
				//sobelG[y][x] = (unsigned char) Gxy;				
			}
		}	
	}
};



behavior cornerDetection( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS],out double sobelGC[IMG_ROWS][IMG_CLMS])
{
	// image buffers these are currently dependent on the size and format of the image used
	unsigned char cor_buffer[IMG_ROWS][IMG_CLMS];  // this can be simplified to save memory, carefully - can update adjecent rows

	int sobelX[3][3] = {{-1, 0, 1}, {-2, 0, 2}, {-1, 0, 1}};
	int sobelY[3][3] = {{1, 2, 1}, {0, 0, 0}, {-1, -2, -1}};
  // Harris parameters        
        //float k = 0.04f;
        float threshold = 50000;

        // Non-maximum suppression parameters
         int r = 3;

        // Gaussian smoothing parameters
         double sigma = 1.2;
         //float[] kernel;
         int size = 7;

         const float eps = 1.1920929E-07f;
   
	


	 int width = IMG_ROWS;
         int height = IMG_CLMS;   
	
	
	// detection variables
	int i, j,x, y,u,v,k,l,maxY,maxX,count;
	float currentValue;
	double min, max, luma;
	//Points_Count=0;

	//Map points 
 	float map[IMG_ROWS][IMG_CLMS];
		


	void main(void) 
	{		
		//********************
		// Perform corner detection
		//********************
		 
                //float* dx = diffx;
               // float* dy = diffy;
                //float* dxy = diffxy;
                //float* H = map;
                float M,A, B, C;

		//Smoothen Using Gaussian Filtering

      for (u = 1; u < width-1; u++) {
        for (v = 1; v < height-1; v++) {
            A=0;
            B=0;
            C=0;
            for(k=0;k<3;k++)
            {
                for(l=0;l<3;l++)
                {
                    A=A+image8bpp[u+k-1][v+l-1]*sobelX[k][l];   //differentials in the neighbourhood
                    B=B+image8bpp[u+k-1][v+l-1]*sobelX[k][l];	//differentials in the neighbourhood
                }

            }

            C=A*B;
            A=A*A;
            B=B*B;

			


  	map[u][v]=A*B-C*C-0.04*(A+B)*(A+B);


		if (map[u][v] > threshold)
                        {
                            // insert value in the map
			   sobelGC[y][x] = 0xFF;
                        }	

		}

	}
		
         
		// Non Max Suppression - 

            // for each row
            for ( y = r, maxY = height - r; y < maxY; y++)
            {
                // for each pixel
                for ( x = r, maxX = width - r; x < maxX; x++)
                {
                     currentValue = map[y][x];
		     //sobelGC[y][x] = 0x00;

                    // for each windows' row
                    for ( i = -r; (currentValue != 0) && (i <= r); i++)
                    {
                        // for each windows' pixel
                        for ( j = -r; j <= r; j++)
                        {
                            if (map[y + i][x + j] > currentValue)
                            {
                                currentValue = 0;
				//sobelGC[y][x] = 0x00;
                                break;
                            }
                        }
                    }

                    // check if this point is really interesting - then it is a corner
                    if (currentValue != 0)
                    {
                        //pointX[count] = x;  //mark the corner
			//pointY[count] = y; 
			//count++;
			sobelGC[y][x] = 0xFF;
			//printf(" Corner Points %d  %d  \n ", x,y);
			PointX[count] = x;
			PointX[count] = y;
			Points_Count ++;

                    }
                }
            }

	//OutQueue.send(cor_buffer[row], (IMG_CLMS));
                            
	printf(" Corner Points %d   \n ", Points_Count);
	}//end of main
	

};





/*
* used to directly write out an image after normalizaing for debug purposes
*/
behavior debugSender(in double debug[IMG_ROWS][IMG_CLMS]) 
{
	// File names
	char OUTPUT_BMP[] = "output8bpp.bmp"; // output file name
	
	int row, wt_cnt, i, j;
	unsigned char pixel;
	double max, min, normFact;
	unsigned char edgeImage[IMG_ROWS][IMG_CLMS];
		
	void main(void) {
		
		//File variable
		FILE * wptr;
		
		// normalize not yet used 
		// find min an max value
		max = 0;
		min = 0;
		for(i = 0; i < IMG_ROWS; i++) 
		{
			for(j = 0; j < IMG_CLMS; j++)
			{
				if ( debug[i][j] > max) max =  debug[i][j];
				if ( debug[i][j] < min) min =  debug[i][j];
			}
		}
		
		normFact = (max - min) / 255;
  
		// open output file file
		wptr = fopen(OUTPUT_BMP, "a"); // append to file which already has header from stimulus
  

		for(row = 0; row < IMG_ROWS; row++)
		{
			
			for(wt_cnt = 0; wt_cnt < IMG_CLMS; wt_cnt++)
			{
				pixel = (unsigned char) (( (debug[row][wt_cnt]) - min) / normFact);
				fprintf(wptr,"%c",pixel);                                    
				
				edgeImage[row][wt_cnt] = pixel;
			}
			
		}
  
	fclose(wptr);
	printf("debug image completed check file %s\n", OUTPUT_BMP);
	}
};



/*
* Normalizes image of array double and sends out pixels over c_queue, assumes
* image pixels are magnitude only, no negative values
*/
behavior normalizeSender(in double diffxy[IMG_ROWS][IMG_CLMS],i_sender OutQueue) 
{
	int row, wt_cnt, i, j;
	int cr,cc,count;
	double val, max, normFact;
	unsigned char edgeImage[IMG_ROWS][IMG_CLMS];		
	void main(void) {
		
		// normalize not yet used 
		// find min an max value
		max = 0;
		//min = 0;
		for(i = 0; i < IMG_ROWS; i++) 
		{
			for(j = 0; j < IMG_CLMS; j++)
			{
				if (diffxy[i][j] > max) max = diffxy[i][j];
			}
		}
		
		normFact = max / 255;

		for(row = 0; row < IMG_ROWS; row++)
		{
			
			for(wt_cnt = 0; wt_cnt < IMG_CLMS; wt_cnt++)
			{
				//edgeImage[row][wt_cnt] = (unsigned char) ((diffxy[row][wt_cnt]) / normFact);
				edgeImage[row][wt_cnt] = (unsigned char) ((diffxy[row][wt_cnt]));
				
				
			}	

			/*for(count = 0; count < Points_Count; count++)
			{
				cr = PointX[count];
				cc = PointX[count];
				edgeImage[cr][cc] = 0xFF;				
				
			}	*/					
			
			// send image out
			OutQueue.send(edgeImage[row], (IMG_CLMS));
		}

	printf("Image has been sent out\n");
	}
};



behavior edgeDetect(i_receiver InQueue, i_sender OutQueue)
{
	// image buffers these are currently dependent on the size and format of the image used
	unsigned char img_buf[IMG_ROWS][IMG_CLMS];
	double sobelG[IMG_ROWS][IMG_CLMS];
	double sobelGC[IMG_ROWS][IMG_CLMS];
	double diffxy[IMG_ROWS][IMG_CLMS];

	//int pointX[IMG_ROWS];
	//int pointY[IMG_CLMS];
	
	receiveImage    imgRcvr(InQueue, img_buf);
	//preProcessImage preProc(img_buf, diffxy);
	//debugSender	dbgSndr(diffxy);
	//sobelEdge sobel(img_buf, sobelGC);
	cornerDetection corner(img_buf,sobelGC);	
	normalizeSender     imgSender(sobelGC,OutQueue);
	

	void main(void) 
	{	
		imgRcvr.main();
		//preProc.main();
		//dbgSndr.main();
		//sobel.main();
		corner.main();
		imgSender.main();
	}
	
};
